#define LED             PORTC.B2
#define LED_DIR         TRISC.B2

#define PID_err_integral_min  2
#define PID_timeslice         0.002 //2ms
#define PID_output_min        0
#define PID_output_max        255

#define PID_Kp                0.7
#define PID_Kd                0.01
#define PID_Ki                0.2

//#define PID_Kp                0.1
//#define PID_Kd                0.01
//#define PID_Ki                0.005

#include "lib_bma180.h"
#include "lib_itg3200.h"
#include "tric-newsensors.h"

int acc[3];
int gyro[3];
float accf[3];
float tau=0.05;
float aa=0.0;
float dt=0.015;
unsigned short RX_values[4];                        // Radio receiver acquired values
           float modulo, roll, pitch, yaw;
           
/* Smarty function to send on RS232 a char String
 */
void Soft_UART_Write_Text(char* testo) {
  char cnt;
  for (cnt=0; cnt<strlen(testo); cnt++) {
      Soft_UART_Write(testo[cnt]);
  }
}

char *strstrip(char *s)
{
    char *end;
    end = s + strlen(s) - 1;
    while (end >= s && isspace(*end))
        end--;
    *(end + 1) = '\0';

    while (*s && isspace(*s))
        s++;

    return s;
}

/* PID data processing
 * Calculate proportional, intergrative and derivative error
 * on each axis to get motors speed
 */
int PID_update() {
    static float PID_err_proportional_old[2];
    static float PID_err_integral[2];
    float PID_err_proportional[2];
    float PID_err_derivative[2];
    int PID_output[2];
    unsigned short cnt;

    cnt=0;
    RX_values[0]=0;
    //for (cnt=0; cnt<2; cnt++) {
        //Caculate PID errors
        PID_err_proportional[cnt] = RX_values[cnt] - roll;  // Need to be implemented conversion between RX and SENSORS values!!!
        if((PID_err_proportional[cnt] > PID_err_integral_min) || (PID_err_proportional[cnt] < -PID_err_integral_min)) PID_err_integral[cnt] += PID_err_proportional[cnt]* dt;
        PID_err_integral[cnt] += PID_err_proportional[cnt] * dt;
        PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) / dt;
        PID_output[cnt] = (int)( (PID_Kp*PID_err_proportional[cnt]) + (PID_Ki*PID_err_integral[cnt]) + (PID_Kd*PID_err_derivative[cnt]) );
        //Saturation Filter
        if(PID_output[cnt] > PID_output_max) PID_output[cnt] = PID_output_max;
        else if(PID_output[cnt] < PID_output_min) PID_output[cnt] = PID_output_min;
        //Update error
        PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
    //}
    return PID_output[0];
}

float Complementary(float oldAngle, float newAngle, float newRate) {
    aa=tau/(tau+dt);
    //aa=0.86;
    oldAngle= aa* (oldAngle + newRate * dt) + (1-aa) * (newAngle);
    return oldAngle;
}

void main() {
    char x_string[8];
    char y_string[8];
    char z_string[8];
    char v_string[8];
    
    LED_DIR = 0;      // LED is set as OUTPUT

    ADCON1 = 0x0F;    // Set A/D Disabled
    CMCON = 0xCF;     // Set Comparator Disabled

    Soft_UART_Init(&PORTB, 2, 3, 115200, 0);

    Delay_ms(500);
    I2C1_Init(400000);
    Delay_ms(500);
    LED=1;
    
    BMA180_init();
    Delay_ms(500);
    ITG3200_init();
    Delay_ms(500);
    LED=0;
    
    while (1) {
           LED = !LED;

           BMA180_readAcc();
           ITG3200_readGyro();
           
           gyro[0] = gyro[0] / 14.375;
           gyro[1] = gyro[1] / 14.375;
           gyro[2] = gyro[2] / 14.375;
           
           accf[0] = acc[0] / 4096.0;
           accf[1] = acc[1] / 4096.0;
           accf[2] = acc[2] / 4096.0;
           
           modulo = sqrt(accf[0]*accf[0]+accf[1]*accf[1]+accf[2]*accf[2]);
           roll = Complementary(roll, asin(accf[1]/modulo)*57.295779, -gyro[0]);
           pitch = Complementary(pitch, asin(accf[0]/modulo)*57.295779, gyro[1]);
           yaw += (gyro[2]*dt);

           RX_values[0] = 0;

           IntToStr(PID_update(), v_string);
           IntToStr(floor(roll), x_string);
           IntToStr(floor(asin(accf[1]/modulo)*57.295779), y_string);
           IntToStr(floor(yaw), z_string);

           //Soft_UART_Write_Text("AccX: ");
           Soft_UART_Write_Text("100,");
           Soft_UART_Write_Text(&x_string);
           Soft_UART_Write_Text(",");
           Soft_UART_Write_Text(&y_string);
           Soft_UART_Write_Text(",");
           Soft_UART_Write_Text(&z_string);
           Soft_UART_Write_Text(",");
           Soft_UART_Write_Text(&v_string);
           //Soft_UART_Write(13);
           Soft_UART_Write(10);
           /*
           IntToStr(floor(gyro[0]*dt), x_string);
           IntToStr(floor(gyro[1]*dt), y_string);
           IntToStr(floor(gyro[2]*dt), z_string);

           Soft_UART_Write_Text(",");
           Soft_UART_Write_Text(&x_string);
           Soft_UART_Write_Text(",");
           Soft_UART_Write_Text(&y_string);
           Soft_UART_Write_Text(",");
           Soft_UART_Write_Text(&z_string);
           Soft_UART_Write(13);
           //Soft_UART_Write(10);
           */
           Delay_ms(20);
    }
}